import numpy as np
import math
import sys
sys.path.append('../CWJ')
from Source.CWJ.gdpz import dian2, intersection_to_point, intersection_to_mian
from Source.CWJ.gdzbzh import pzjc
from Source.CWJ.qyhf import check


def qiudian(point1, point2, long, angle):
    p = np.zeros(2)
    for i in range(2):
        p[i] = point2[i] - point1[i]
    a = (p[0] ** 2 + p[1] ** 2) ** 0.5
    if p[0] != 0:
        k = p[1] / p[0]  # 直线1的斜率
    if angle == 1:  # 两直线垂直
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal[0] = point2[0] + long * (p[1] / a)
        goal1[0] = point2[0] - long * (p[1] / a)
        goal[1] = point2[1] - long * (p[0] / a)
        goal1[1] = point2[1] + long * (p[0] / a)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        # print(goal,goal1)
        return goal, goal1
    if angle == 2:  # 夹角为45度时
        if p[0] == 0:
            k = 0
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal2 = np.zeros(3)
        goal3 = np.zeros(3)
        if k == 1 or k == -1:
            # 求出x，y坐标
            goal[0] = point2[0] - long
            goal[1] = point2[1]
            goal1[0] = point2[0] + long
            goal1[1] = point2[1]
            goal2[0] = point2[0]
            goal2[1] = point2[1] - long
            goal3[0] = point2[0]
            goal3[1] = point2[1] + long
        else:

            k1 = (1 + k) / (1 - k)
            a = (k1 ** 2 + 1) ** 0.5
            goal[0] = point2[0] + long * (1 / a)
            goal1[0] = point2[0] - long * (1 / a)
            goal[1] = point2[1] - long * (k1 / a)
            goal1[1] = point2[1] + long * (k1 / a)
            k2 = (k - 1) / (1 + k)
            b = (k2 ** 2 + 1) ** 0.5
            goal2[0] = point2[0] + long * (1 / b)
            goal3[0] = point2[0] - long * (1 / b)
            goal2[1] = point2[1] - long * (k2 / b)
            goal3[1] = point2[1] + long * (k2 / b)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        goal2[2] = point2[2]
        goal3[2] = point2[2]
        # print(goal,goal1,goal2,goal3)
        return [goal, goal1, goal2, goal3]

def zuoyou(coor, stru):
    cx = []
    cy = []
    sx = []
    sy = []
    for i in range(0, len(coor)):
        cx.append(coor[i][0])
        cy.append(coor[i][1])
    for j in range(0, len(stru)):
        sx.append(stru[j][0])
        sy.append(stru[j][1])
    cmax = [max(cx), max(cy)]
    smax = [max(sx), max(sy)]
    cmin = [min(cx), min(cy)]
    smin = [min(sx), min(sy)]
    dis1 = math.sqrt((cmax[0] - smax[0]) ** 2 + (cmax[1] - smax[1]) ** 2)
    dis2 = math.sqrt((cmin[0] - smin[0]) ** 2 + (cmin[1] - smin[1]) ** 2)
    return dis2 - dis1


# 输入碰撞坐标和管道流，输出碰撞的平均坐标
def Average_value(coor, pi):
    index = len(coor)
    x = 0
    y = 0
    z = 0
    for i in range(index):
        x = x + coor[i][0]
        y = y + coor[i][1]
        z = z + coor[i][2]
    x = x / index
    y = y / index
    z = z / index
    xyz = [x, y, z]
    redis = pi[3] / 2  # 回退的长度为半个障碍物的宽度
    step = [redis / x, redis / y, redis / z]  # 步长
    for j in range(0, 3):
        if pi[1][j] - pi[0][j] == 0:
            xyz[j] = pi[0][j]
        else:
            if pi[1][j] >= pi[0][j]:
                xyz[j] = (xyz[j] * (1 - step[j]))
            else:
                xyz[j] = (xyz[j] * (1 + step[j]))
    xy = [x - xyz[0], y - xyz[1]]
    return [xyz, xy]


# 上下型障碍物
# 输入管道流和碰撞坐标，输出避障后的管道流
# pi: 管道流
# stru: 上下型障碍物表达式（与管道表示相同
# temp: 上下型障碍物的类型
# list1: 附近障碍物的列表
def Avoid_crossbeam(pi, stru, list1, temp):
    coor = intersection_to_point(pi, stru)  # 碰撞坐标
    m = 0
    n = 0
    xyz = Average_value(coor, pi)
    high = stru[3]  # 障碍物的高度
    stru1 = dian2(stru[0], stru[1], stru[2], stru[3])
    # lenp：管道回退的距离
    # d1: 回退的距离
    lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2)
    d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2)
    # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
    j1 = int(lenp / d1)
    if temp == 'Frame':  # 如果是横梁，向下
        for j in range(0, j1 + 1):
            new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
            for i in range(1, 9):
                new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] - 0.25 * i * high]
                pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向下的直管
                a = pzjc([pi2], [stru])
                if a == [[]]:
                    pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                    b = pzjc([pi3], [stru])
                    if b == [[]]:
                        final_pi = [new_pi, pi2, pi3]
                        # print(final_pi)
                        m = 1
                        for z in range(0, len(list1)):
                            # stru2 = dian2(list1[z][0], list1[z][1], list1[z][2], list1[z][3])
                            d = pzjc(final_pi, [[list1[z][0], list1[z][1], list1[z][2], list1[z][3]]])
                            if d == [[], [], []]:
                                n = n + 1
                            else:
                                continue
                        if n == len(list1):  # 绕行完成
                            break
                        else:  # 向下绕行有障碍物，直接穿过
                            m = 3
                            final_pi = pi
                            break

                    if i == 8:
                        m = 2
                        final_pi = pi
                        break
                else:
                    xyz1 = Average_value([xyz[0]], pi)
                    xyz[0] = xyz1[0]
                    if j == j1:
                        final_pi = pi
                    break

            if m == 1 or m == 2 or m == 3:
                break


    else:  # 否则是管道或其他
        # 向上
        for j in range(0, j1 + 1):
            new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
            for i in range(1, 9):
                new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] + 0.25 * i * high]
                pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向上的直管
                a = pzjc([pi2], [stru])
                if a == [[]]:
                    pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                    b = pzjc([pi3], [stru])
                    if b == [[]]:
                        final_pi = [new_pi, pi2, pi3]
                        m = 1
                        for z in range(0, len(list1)):
                            # stru2 = dian2(list1[z][0], list1[z][1], list1[z][2], list1[z][3])
                            d = pzjc(final_pi, [[list1[z][0], list1[z][1], list1[z][2], list1[z][3]]])
                            if d == [[], [], []]:
                                n = n + 1
                            else:
                                continue
                        if n == len(list1):  # 绕行完成
                            break
                        else:  # 向上绕行有障碍物，向下绕
                            m = 3
                            final_pi = Avoid_crossbeam(pi, stru, list1, 'Frame')  # temp改为横梁型
                            break
                    if i == 8:
                        m = 2
                        final_pi = pi
                        # final_pi = Avoid_crossbeam(pi, stru, list1, 'Frame')
                        break
                else:
                    xyz1 = Average_value([xyz[0]], pi)
                    xyz[0] = xyz1[0]
                    if j == j1:
                        final_pi = pi
                    break

            if m == 1 or m == 2 or m == 3:
                break

    return final_pi


# 左右型障碍物
# 输入管道流，碰撞坐标和障碍物类型，输出避障后的管道流
# coor：碰撞点坐标
# pi: 管道流
# column: 上下型障碍物表达式（与管道流表达相同
# stru: 障碍物产生碰撞的面上的四点坐标
def Avoid_column(pi, column, list1, temp=0):
    coor = intersection_to_point(pi, column)
    stru = intersection_to_mian(pi, column)
    index = 0
    m = 0
    n = 0
    wide = column[2]  # 障碍物宽度
    xyz = Average_value(coor, pi)
    co1 = dian2(column[0], column[1], column[2], column[3])
    # 管道终点 距离 碰撞平均坐标的长度
    lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2)
    d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2)
    # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
    j1 = int(lenp / d1)
    if temp == 0:
        dis = zuoyou(coor, stru)
    else:
        dis = temp
        # #point1管道1起点，point2管道1终点，long管道2长度，angle夹角angle=1为垂直，angle=2为45度夹角

    if dis >= 0:  # 向右拐
        # ①
        for j in range(0, j1 + 1):
            len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2)  # 管道终点到回退点的距离
            new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
            # 横向增加的长度取两个wide
            # ②
            for i in range(1, 9):
                a = qiudian(new_pi[0], new_pi[1], (i / 4) * wide, 1)
                list11 = a[0].tolist()
                pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                test1 = pzjc([pi2], [column])
                # ③
                if test1 == [[]]:
                    b = qiudian(pi2[0], pi2[1], len_pi, 1)
                    list2 = b[1].tolist()
                    pi3 = [pi2[1], list2, pi[2], pi[3]]
                    a1 = pzjc([pi3], [column])
                    # ④
                    if a1 == [[]]:
                        m = 1
                        final_pi = [new_pi, pi2, pi3]
                        # 遍历附近所有障碍物
                        # ⑤
                        for z in range(0, len(list1)):
                            # co2 = dian2(list1[z][0], list1[z][1], list1[z][2], list1[z][3])
                            d = pzjc(final_pi, [[list1[z][0], list1[z][1], list1[z][2], list1[z][3]]])
                            if d == [[], [], []]:
                                n = n + 1
                            else:
                                continue
                        if n == len(list1):  # 绕行完成
                            break
                        else:  # 向右绕行有障碍物，尝试向左
                            m = 3
                            if temp == 0:
                                final_pi = Avoid_column(pi, column, list1, -1)
                            else:
                                index = 1
                            break
                    # ④
                    else:
                        if i == 8:
                            m = 2
                            index = 1
                        continue
                # ③
                else:
                    xyz1 = Average_value([xyz[0]], pi)
                    xyz[0] = xyz1[0]
                    if j == j1:
                        index = 1
                    break

            # 若m = 1， 说明已经完成了绕行，直接退出①
            # 若m = 2， 说明pi2可以绕过，pi3不行，回退也无法绕行，直接退出①
            # 若m = 3， 说明向右绕行成功，但是会撞上其他障碍物，直接退出①
            if m == 1 or m == 2 or m == 3:
                break




    else:  # 向左
        for j in range(0, j1 + 1):
            len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2)
            new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
            for i in range(1, 9):
                a = qiudian(new_pi[0], new_pi[1], (i / 4) * wide, 1)
                list11 = a[1].tolist()
                pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                test1 = pzjc([pi2], [column])
                if test1 == [[]]:
                    b = qiudian(pi2[0], pi2[1], len_pi, 1)
                    list2 = b[0].tolist()
                    pi3 = [pi2[1], list2, pi[2], pi[3]]
                    a1 = pzjc([pi3], [column])
                    if a1 == [[]]:
                        m = 1
                        final_pi = [new_pi, pi2, pi3]
                        for z in range(0, len(list1)):
                            # co2 = dian2(list1[z][0], list1[z][1], list1[z][2], list1[z][3])
                            d = pzjc(final_pi, [[list1[z][0], list1[z][1], list1[z][2], list1[z][3]]])
                            if d == [[], [], []]:
                                n = n + 1
                            else:
                                continue
                        if n == len(list1):  # 绕行完成
                            break
                        else:  # 向右绕行有障碍物
                            m = 3
                            if temp == 0:
                                final_pi = Avoid_column(pi, column, list1, 1)
                            else:
                                index = 1
                            break
                    else:
                        if i == 8:
                            m = 2
                            index = 1
                        continue
                else:
                    xyz1 = Average_value([xyz[0]], pi)
                    xyz[0] = xyz1[0]
                    if j == j1:
                        index = 1
                    break
            if m == 1 or m == 2 or m == 3:
                break

        if m == 3 and temp == 0:
            final_pi = Avoid_column(pi, column, list1, 1)

    if index == 1:  # 左右都有障碍物 从中穿过
        final_pi = pi

    return final_pi


# temp: 障碍物的属性
# pi： 管道坐标
# stru: 障碍物表达式
# list1: 附近障碍物列表
def Avoid_obstacles(pi, stru, list1, temp):
    # if stru[1][2] - stru[0][2] < 50:  # 说明是横着的
    if temp == 'Frame':
        new_pi = Avoid_crossbeam(pi, stru, list1, temp)
    elif temp == 'Column':
        new_pi = Avoid_column(pi, stru, list1)
    elif temp == 'Pipe':
        if abs(stru[1][2] - stru[0][2]) < 50:
            new_pi = Avoid_crossbeam(pi, stru, list1, 'Pipe')
        else:
            new_pi = Avoid_column(pi, stru, list1)
    else:
        new_pi = pi
    return new_pi



# 输入管道，障碍物列表，障碍物类型，输出避障后的管道
# 需完善：temp为障碍物类型，可考虑在函数中获取障碍物的类型，而不作为参数使用
def Pi_bypass(pi, list1, temp):
    if not list1:
        print("no blocks")
        return pi
    for j in range(0, len(pi)):
        for i in range(0, len(list1)):
            a = pzjc([pi[j]], [[list1[i][0], list1[i][1], list1[i][2], list1[i][3]]])
            if a != [[]]:

                pi1 = Avoid_obstacles(pi[j], list1[i], list1, temp)
                if pi1 != pi[j]:
                    pi.pop(j)
                    for z in range(0, len(pi1)):
                        pi.insert(j + z, pi1[z])
                        if j != len(pi) - 1:
                            if z != len(pi) - 1:
                                pi[z + 1][0] = pi[z][1]
                    break
                else:
                    break

            else:
                continue
    print('避障后的管道', pi)
    return pi


# pi = [[[100, 1000, 1000], [600, 1000, 1000], 100, 100], [[600, 1000, 1000], [600, 500, 1000], 100, 100],
#       [[600, 500, 1000], [1200, 500, 1000], 100, 100]]
# list1 = [[[300, 1000, 0], [300, 1000, 1200], 100, 100], [[300, 800, 0], [300, 800, 1000], 100, 100]]
# list2 = [[]]
# list3 = [[[900, 100, 1000], [900, 1200, 1000], 100, 100]]
#
# print(pi)
# list4 = [list1, list3]
# pi = Pi_bypass(pi, list4[0], 'Column')
# pi = Pi_bypass(pi, list4[1], 'Frame')
# print(pi)

# # list5 = [list1, list3, pi]
# # pi2 = [[[750, 100, 1000], [750, 1500, 1000], 50, 50], [[100, 1500, 1000], [750, 1500, 1000], 50, 50]]
# pi2 = [[[100, 1500, 1000], [750, 1500, 1000], 50, 50], [[750, 1500, 1000], [750, 100, 1000], 50, 50]]
#
# dom = xml.dom.minidom.parse('D:/学习/实验室/ARAI/python_code/RevitData.xml')
#
# # 得到文档元素对象
# root = dom.documentElement
#
# # 读出障碍物的类型
# a = root.getElementsByTagName('Entity')
# for i in range(0, len(a)):
#     item1 = a[i]
#     un1 = item1.getAttribute("type")
#     # print(un1)
#
# pi2 = Pi_bypass(pi2, list4[0], 'Column')
# pi2 = Pi_bypass(pi2, list4[1], 'Frame')
# pi2 = Pi_bypass(pi2, pi, 'Pipe')
# print(pi2)

# for i in range(, ？):
#   newpi = Pi_bypass(pi, 障碍物列表)


# pi = [[[[140305.419194607, 163957.456537679, 55643.2], [143355.419194607, 163957.456537679, 55643.2], 150.0, 150.0], [[143505.419194607, 163807.456537679, 55643.2], [143505.419194607, 163176.956537679, 55643.2], 150.0, 150.0], [[143619.919194607, 162957.456537679, 55643.2], [145307.338830655, 162957.456537679, 55643.19109296], 100.0, 100.0]], [[[145419.338830655, 162835.456537679, 55643.19109296], [145419.338830655, 161951.956537674, 55643.1989781], 100.0, 100.0], [[145533.838830655, 161837.456537679, 55643.2], [147023.526770259, 161837.456537679, 55643.2], 100.0, 100.0]], [[[145541.338830655, 162957.456537679, 55643.19109296], [147023.526770263, 162957.456537679, 55643.199354479], 100.0, 100.0]], [[[145419.338830655, 163079.456537679, 55643.19109296], [145423.091076715, 164063.160284932, 55643.199073353], 100.0, 100.0], [[145537.750753206, 164177.383436921, 55643.2], [147023.526770259, 164177.383436921, 55643.2], 100.0, 100.0]], [[[123922.049093157, 164984.83448502, 55643.2], [126514.049093157, 164984.83448502, 55643.2], 305.0, 305.0], [[126822.049093157, 164676.83448502, 55643.2], [126822.049093157, 163910.225357985, 55643.2], 305.0, 305.0], [[127075.049093157, 163584.83448502, 55643.2], [129647.049093157, 163584.83448502, 55643.2], 250.0, 250.0]], [[[130022.049093157, 163109.83448502, 55643.2], [130022.049093157, 162159.83448502, 55643.2], 250.0, 250.0]], [[[129647.049093157, 161684.83448502, 55643.2], [123922.049093157, 161684.83448502, 55643.2], 250.0, 250.0]], [[[130397.049093157, 161684.83448502, 55643.2], [133822.049093157, 161684.83448502, 55643.2], 250.0, 250.0]], [[[130397.049093157, 163584.83448502, 55643.2], [133569.049093157, 163584.83448502, 55643.2], 250.0, 250.0], [[133822.049093157, 163837.83448502, 55643.2], [133822.049093157, 165484.83448502, 55643.2], 250.0, 250.0]]]
# a = [[[104771.340897978, 111837.3598301205, 55149.9901836615], [106125.49736686701, 111837.3598301205, 55149.9901836615], 1200.003258259, 7917.556559981], [[51793.534475137, 111837.359470633, 55150.0], [53460.692953922, 111837.359470633, 55150.0], 1200.000000484, 7965.322002556], [[78554.656351634, 137985.70681807198, 55149.999999999505], [79754.656351703, 137985.70681807198, 55149.999999999505], 1352.720338762, 7917.357741925], [[78554.656351653, 85167.3442905985, 55150.0], [79754.656351653, 85167.3442905985, 55150.0], 1658.371947077, 7963.98132277], [[51786.59069023, 101337.359470632, 55150.0], [53462.565193062, 101337.359470632, 55150.0], 1200.000000486, 7966.667141966], [[51795.120812358, 122337.35947063251, 55150.0], [53460.265156067, 122337.35947063251, 55150.0], 1200.000000483, 7965.01468213], [[104772.151833581, 122337.359832725, 55149.9999999995], [106122.521719635, 122337.359832725, 55149.9999999995], 1200.003249148, 7916.999156835], [[104771.534654396, 101337.3598307425, 55149.9999999995], [106124.786655587, 101337.3598307425, 55149.9999999995], 1200.003256083, 7917.438314285], [[68054.656351633, 137985.91100021653, 55150.0], [69254.65635170201, 137985.91100021653, 55150.0], 1353.434597099, 7917.466577494], [[89054.656351634, 137985.6143733695, 55150.0], [90254.656351703, 137985.6143733695, 55150.0], 1352.396950733, 7917.308465774], [[68054.656351653, 85170.4970387545, 55149.999999999505], [69254.656351653, 85170.4970387545, 55149.999999999505], 1647.406956921, 7962.308197399], [[89054.656351653, 85168.392439937, 55150.0], [90254.656351653, 85168.392439937, 55150.0], 1654.726830306, 7963.425127456], [[104885.172272608, 90837.35969033101, 55149.999999999505], [105965.260523943, 90837.35969033101, 55149.999999999505], 1000.002598818, 7339.362738501], [[52225.372336441, 132836.4651450405, 55150.000000000495], [53610.255464327005, 132836.4651450405, 55150.000000000495], 1000.001523347, 7381.062263227]]

# listx = [[[348593.065319341, 120641.158927369, 56900.0], [348593.065319341, 120641.158927369, 55568.9], 100.0, 100.0], [[349243.865319341, 120641.158927369, 54918.1], [350693.065319341, 120641.158927369, 54918.1], 100.0, 100.0]]
# pii = [[[350216.954417915, 120562.224099402, 56121.2], [347416.954417915, 120562.224099402, 56168.8], 100.0, 100.0]]
# piii = Pi_bypass(pii, listx, 'Pipe')
# print(piii)
